#ifndef VEL_POSE_CONNECTOR_H
#define VEL_POSE_CONNECTOR_H

#include "ros/ros.h"
#include "tf/tf.h"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"
#include "geometry_msgs/PoseStamped.h"
#include "geometry_msgs/TwistStamped.h"
#include "Eigen/Core"
#include "nav_msgs/Odometry.h"

#define __APP_NAME__ "vel_pose_connector"

class VelPoseConnector{
    public:
        VelPoseConnector();
        ~VelPoseConnector();
        void run();

    private:
        ros::NodeHandle nh_;
        ros::NodeHandle private_nh_;

    private:
        tf2_ros::Buffer buffer_;
        tf2_ros::TransformListener listener_;

    private:
        std::string output_pose_topic_;
        std::string output_velocity_topic_;
        std::string output_odom_topic_;
    
    private:
        ros::Publisher pose_pub_;
        ros::Publisher velocity_pub_;
        ros::Publisher odom_pub_;
    
    private:
        std::shared_ptr<geometry_msgs::PoseStamped> ref_pose_;
        geometry_msgs::TwistStamped ts_;
    
    private:
        void process(const ros::TimerEvent& e);
        double delta_yaw(const double &orin_yaw, const double &yaw);
};

#endif